
// local
#include "q6cache.h"
// #include "FFTTools.hpp"
#include "RectTools.hpp"

#include "fastcv_aux.hpp"

#include "TrackerKCF.hpp"

#define FARF_HIGH 1
#include <HAP_farf.h>

#if defined(V5_4)
#undef V5_4
#endif
#include "hvx_vector_register_defines.h"
extern "C" {
    extern int32_t qhblas_hvx_uw_vector_dot_aub( uint8_t *restrict input_1, uint8_t *restrict input_2, uint32_t *restrict output, uint32_t size);
    extern void transpose_vshuff_int64(const int64_t* src, int64_t* dst, int matrix_order, int stride);
    extern void transpose_vshuff_int32(const int32_t* src, int32_t* dst, int matrix_order, int stride);
}
using namespace std;


bool gDbg = true;
// namespace handtrack {

//////////////
/** Public **/
//////////////
TrackerKCF::TrackerKCF(int patch_size, bool multiscale)
    : _patch_size(patch_size),
      _sigma(0.2f),
      _output_sigma_factor(0.125f),
      _lambda(wfq16_from_float(0.0001f)),
      _padding(3.f),
      _scale_step(1.f),
      _scale_weight(0.f),
      _fft(patch_size){ 
    _neg_sigma2 = wfq15_from_float(qhmath_div_f(-1.f, _sigma * _sigma)); // float --> Q15
    if (multiscale) { 
        this->_scale_step = 1.1f;
        this->_scale_weight = 0.9f;
    }
    debug_buf = (uint8_t*)malloc(patch_size*patch_size*sizeof(int64_t));

    int logN = (int)Q6_R_ct0_R(patch_size);
    FARF(RUNTIME_HIGH, "--------fft gen: np=%d,logN=%d------", patch_size, logN);
    // FARF(RUNTIME_HIGH, "[%f,%fi],[%f,%fi],[%f,%fi]", qhcomplex_creal_f(_r_twiddles1[0]), qhcomplex_cimag_f(_r_twiddles1[0]),
    //     qhcomplex_creal_f(_r_twiddles2[0]), qhcomplex_cimag_f(_r_twiddles2[0]),
    //     qhcomplex_creal_f(_c_twiddles[0]), qhcomplex_cimag_f(_c_twiddles[0]));

    this->createHanningMats();
    this->createGaussianPeak();
    this->_alphaf = CvMat_cwfq16(this->_patch_size, this->_patch_size);
    this->_alphaf.zeros();
    this->_tmpl = CvMat_hfq15(this->_patch_size, this->_patch_size);
    this->_tmpl.zeros();

}

TrackerKCF::~TrackerKCF()
{
    if (debug_buf) {
        free(debug_buf);
    }
}

void TrackerKCF::update(
    const CvRect& src_roi,
    CvMatU8 &src_image,
    float train_interp_factor
)
{
    // printf("=================update=================\n");
    // assert(src_roi.width > 0 && src_roi.height > 0);
    // assert(train_interp_factor >= 0.f && train_interp_factor <= 1.f);

    this->adjustROI(src_roi, src_image);
    CvMat_hfq15 x = this->getFeatures(src_image, 1.f);
    // PRINT_CVMATF32("x", x, 0, 0, 8, 8) 
    CvMat_hfq15 k = this->gaussianCorrelation(x, x);
    // PRINT_CVMATF32("k", k, 0, 0, 8, 8) 

    // cv::Mat alphaf = FFTTools::complexDivision(
    //     this->_prob,
    //     (FFTTools::fftd(k) + this->_lambda)
    // );
    // CvMatComplex kk = FFTTools::fftd(k, this->_twiddles1, this->_twiddles2, this->_twiddles);
    // memcpy(debug_buf, k.data, k.rows*k.cols*sizeof(hfq15_t));
    CvMat_cwfq16 kk(this->_patch_size, this->_patch_size);
    _fft.fftd(k.data, kk.data);
    PRINT_CVMATcwfq16("kk", kk, 0, 0, 8, 8)
    PRINT_CVMATcwfq16("kk", kk, _patch_size/2, _patch_size/2, 8, 8)
    TicToc tic;
#if 0
    // printf("---------lambda:%f\n", this->_lambda);
    // PRINT_CVMATComplex("kk", kk, 0, 0, 8, 8)
    for (int i = 0; i < kk.rows*kk.cols; i++) {
        kk(i) += this->_lambda;
    }
    CvMatF32Complex alphaf = FFTTools::complexDivision( this->_prob, kk);
    // PRINT_CVMATComplex("alphaf", alphaf, 0, 0, 8, 8)
    // this->_tmpl = (1.f - train_interp_factor) * this->_tmpl + train_interp_factor * x;
    // this->_alphaf = (1.f - train_interp_factor) * this->_alphaf + train_interp_factor * alphaf;
    // printf("---------train_interp_factor:%f\n", train_interp_factor);
    float v = 1.f - train_interp_factor;
    for (int i = 0; i < x.rows*x.cols; i++) {
        this->_tmpl(i) = v*this->_tmpl(i) + train_interp_factor*x(i); 
        this->_alphaf(i) = v*this->_alphaf(i) + train_interp_factor*alphaf(i);
    }
#else
    // float v = 1.f - train_interp_factor;
    // for (int i = 0; i < x.rows*x.cols; i++) {
    //     this->_tmpl(i) = v*this->_tmpl(i) + train_interp_factor*x(i); 
    //     float complex v_c = this->_prob(i)/(kk(i)+this->_lambda);
    //     this->_alphaf(i) = v*this->_alphaf(i) + train_interp_factor*v_c;
    // }
    // hfq15_t q15_tif = hfq15_from_float(train_interp_factor);
    // hfq15_t q15_inv_tif = hfq15_const_1() - q15_tif;
#if 0
    wfq16_t q16_tif = wfq16_from_float(train_interp_factor);
    wfq16_t q16_inv_tif = wfq16_const_1() - q16_tif;
    for (int i = 0; i < x.rows*x.cols; i++) {
        this->_tmpl(i) = (wfq16_mul(q16_inv_tif, this->_tmpl(i)<<1) + wfq16_mul(q16_tif, ((wfq16_t)x(i))<<1))>>1; 
    }
#else
    {
        wfq15_t q15_tif = wfq15_from_float(train_interp_factor);
        wfq15_t q15_inv_tif = wfq15_const_1() - q15_tif;
        long long l2fetch1 = CreateL2pfParam(this->_tmpl.colStride(), this->_tmpl.cols, 1, 0);
        long long l2fetch2 = CreateL2pfParam(x.colStride(), x.cols, 1, 0);
        hfq15_t *pSrc1 = this->_tmpl.data;
        hfq15_t *pSrc2 = x.data;
        L2fetch( (unsigned int)(pSrc1), l2fetch1);
        L2fetch( (unsigned int)(pSrc2), l2fetch2);
        HVX_Vector v_tif = Q6_V_vsplat_R(Q6_R_sath_R(q15_tif)<<16); // set tif to MSB
        HVX_Vector v_inv_tif = Q6_V_vsplat_R(q15_inv_tif<<16); // set inv_tif to MSB
        HVX_VP V1__0, V3__2;
        HVX_Vector *p_v1 = (HVX_Vector*)this->_tmpl.data;
        HVX_Vector *p_v2 = (HVX_Vector*)x.data;
        for (int i = 0; i < this->_tmpl.rows; i++) {
            if (i < this->_tmpl.rows-1) {
                L2fetch( (unsigned int)(pSrc1+this->_tmpl.cols), l2fetch1);
                L2fetch( (unsigned int)(pSrc2+x.cols), l2fetch2);
            }
            for (int j = 0; j < this->_tmpl.cols; j += 64) {
                V0 = *p_v1;
                V1_0 = Q6_Ww_vunpack_Vh(V0);
                V0 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V0, v_inv_tif);
                V1 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V1, v_inv_tif);
                V2 = *p_v2++;
                V3_2 = Q6_Ww_vunpack_Vh(V2);
                V2 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V2, v_tif);
                V3 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V3, v_tif);
                V0 = Q6_Vw_vadd_VwVw_sat(V0, V2);
                V1 = Q6_Vw_vadd_VwVw_sat(V1, V3);
                V0 = Q6_Vh_vpack_VwVw_sat(V1, V0);
                *p_v1++ = V0;
            }
        }
    } 
#endif
    // printf("-----------update tmpl: %lluus\n", tic.toc());
    // PRINT_CVMATfq15("_tmpl", this->_tmpl, 0, 0, 8, 8)
    tic.tic();
#if 0
    wfq16_t q16_tif = wfq16_from_float(train_interp_factor);
    wfq16_t q16_inv_tif = wfq16_const_1() - q16_tif;
    CvMat_cwfq16 alpha(this->_patch_size, this->_patch_size);
    for (int i = 0; i < x.rows*x.cols; i++) {
        // this->_tmpl(i) = static_cast<uint8_t>((fq17_mul(q17_inv_tif, ((int32_t)this->_tmpl(i))<<10) + fq17_mul(q17_tif, ((int32_t)x(i))<<10))>>10); 
        cwfq16_t c_v = cwfq16_add_re(kk(i), this->_lambda);
        c_v = cwfq16_div(this->_prob(i), c_v);
        alpha(i) = c_v;
        this->_alphaf(i) = cwfq16_mul_scale(this->_alphaf(i), q16_inv_tif) + cwfq16_mul_scale(c_v, q16_tif);
    }
    PRINT_CVMATcwfq16("alphaf", alpha, 0, 0, 8, 8)
    PRINT_CVMATcwfq16("alphaf", alpha, _patch_size/2, _patch_size/2, 8, 8)
#else
    {
        CvMat_cwfq16 c_v(kk.rows, kk.cols);

        long long l2fetch1 = CreateL2pfParam(kk.colStride(), kk.colStride(), 1, 0);
        long long l2fetch2 = CreateL2pfParam(this->_prob.colStride(), this->_prob.colStride(), 1, 0);
        cwfq16_t *pSrc1 = kk.data;
        cwfq16_t *pSrc2 = this->_prob.data;
        L2fetch( (unsigned int)(pSrc1), l2fetch1);
        L2fetch( (unsigned int)(pSrc2), l2fetch2);
        HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8, V11__10, V13__12, V15__14, V17__16;
        // HVX_VectorPred Qp;
        HVX_VectorPair *p_pv1 = (HVX_VectorPair*)kk.data;
        HVX_VectorPair *p_pv2 = (HVX_VectorPair*)this->_prob.data;
        HVX_Vector v_lambda = Q6_V_vsplat_R(this->_lambda);
        int32_t __attribute__ ((__aligned__(128))) real_buff[32];
        int32_t __attribute__ ((__aligned__(128))) imag_buff[32];
        int32_t __attribute__ ((__aligned__(128))) deno_buff[32];
        uint32_t shift = (this->_patch_size == 64) ? 8 : 6;
        for (int i = 0; i < kk.rows; i++) {
            if (i < kk.rows-1) {
                L2fetch( (unsigned int)(pSrc1+kk.cols), l2fetch1);
                L2fetch( (unsigned int)(pSrc2+this->_prob.cols), l2fetch2);
            }

            for (int j = 0; j < kk.cols; j += 32) {
                V1_0 = *p_pv2++;
                V3_2 = Q6_W_vdeal_VVR(V1, V0, -4); // V2: real, V3: imag 
                V1_0 = *p_pv1++;
                V5_4 = Q6_W_vdeal_VVR(V1, V0, -4); // V4: real, V5: imag 
                V4 = Q6_Vw_vadd_VwVw(V4, v_lambda); // c_v
                //(R1+I1i)/(R2+I2i)=(R1*R2+I1*I2)/(R2*R2+I2*I2)+(R2*I1-R1*I2)/(R2*R2+I2*I2)i
                // R1*R2 // int32
                // V7 = Q6_Vw_vmpye_VwVuh(V4, V2); 
                // V7 = Q6_Vw_vmpyoacc_VwVwVh_s1_sat_shift(V7, V4, V2);
                V7_6 = Q6_W_vmpye_VwVuh(V4, V2); 
                V7_6 = Q6_W_vmpyoacc_WVwVh(V7_6, V4, V2);
                V6 = Q6_Vuw_vlsr_VuwR(V6, 32-shift);
                V7 = Q6_Vw_vasl_VwR(V7, shift);
                V7 = Q6_Vw_vadd_VwVw_sat(V6, V7);
                // I1*I2 // int32
                // V9 = Q6_Vw_vmpye_VwVuh(V5, V3); 
                // V9 = Q6_Vw_vmpyoacc_VwVwVh_s1_sat_shift(V9, V5, V3);
                V9_8 = Q6_W_vmpye_VwVuh(V5, V3); 
                V9_8 = Q6_W_vmpyoacc_WVwVh(V9_8, V5, V3);
                V8 = Q6_Vuw_vlsr_VuwR(V8, 32-shift);
                V9 = Q6_Vw_vasl_VwR(V9, shift);
                V9 = Q6_Vw_vadd_VwVw_sat(V8, V9);
                
                // R1*I2 // int32
                // V11 = Q6_Vw_vmpye_VwVuh(V2, V5); 
                // V11 = Q6_Vw_vmpyoacc_VwVwVh_s1_sat_shift(V11, V2, V5);
                V11_10 = Q6_W_vmpye_VwVuh(V2, V5); 
                V11_10 = Q6_W_vmpyoacc_WVwVh(V11_10, V2, V5);
                V10 = Q6_Vuw_vlsr_VuwR(V10, 32-shift);
                V11 = Q6_Vw_vasl_VwR(V11, shift);
                V11 = Q6_Vw_vadd_VwVw_sat(V10, V11);
                // R2*I1 // int32
                // V13 = Q6_Vw_vmpye_VwVuh(V4, V3); 
                // V13 = Q6_Vw_vmpyoacc_VwVwVh_s1_sat_shift(V13, V4, V3);
                V13_12 = Q6_W_vmpye_VwVuh(V4, V3); 
                V13_12 = Q6_W_vmpyoacc_WVwVh(V13_12, V4, V3);
                V12 = Q6_Vuw_vlsr_VuwR(V12, 32-shift);
                V13 = Q6_Vw_vasl_VwR(V13, shift);
                V13 = Q6_Vw_vadd_VwVw_sat(V12, V13);
                // R2*R2
                // V15 = Q6_Vw_vmpye_VwVuh(V4, V4);
                // V15 = Q6_Vw_vmpyoacc_VwVwVh_s1_sat_shift(V15, V4, V4);
                V15_14 = Q6_W_vmpye_VwVuh(V4, V4);
                V15_14 = Q6_W_vmpyoacc_WVwVh(V15_14, V4, V4);
                V14 = Q6_Vuw_vlsr_VuwR(V14, 32-shift);
                V15 = Q6_Vw_vasl_VwR(V15, shift);
                V15 = Q6_Vw_vadd_VwVw_sat(V14, V15);
                // I2*I2
                // V17 = Q6_Vw_vmpye_VwVuh(V5, V5);
                // V17 = Q6_Vw_vmpyoacc_VwVwVh_s1_sat_shift(V17, V5, V5);
                V17_16 = Q6_W_vmpye_VwVuh(V5, V5);
                V17_16 = Q6_W_vmpyoacc_WVwVh(V17_16, V5, V5);
                V16 = Q6_Vuw_vlsr_VuwR(V16, 26);
                V17 = Q6_Vw_vasl_VwR(V17, shift);
                V17 = Q6_Vw_vadd_VwVw_sat(V16, V17);

                // R2*R2+I2*I2
                V0 = Q6_Vw_vadd_VwVw_sat(V15, V17);
                *(HVX_Vector*)deno_buff = V0; // coef
                // R1*R2+I1*I2  // real part
                V0 = Q6_Vw_vadd_VwVw_sat(V7, V9);
                *(HVX_Vector*)real_buff = V0; // real part
                // int64_t subtract
                // Qp = Q6_Q_vcmp_gt_VuwVuw(V12, V10);
                // V0 = Q6_Vw_vsub_VwVw(V12, V10); // lo part
                // V11 = Q6_Vw_condacc_QnVwVw(Qp, V11, v_one);
                // V1 = Q6_Vw_vsub_VwVw_sat(V13, V11); // hi part
                // R2*I1-R1*I2  // image part
                V0 = Q6_Vw_vsub_VwVw_sat(V13, V11); // hi
                *(HVX_Vector*)imag_buff = V0; // image part 
                // TODO: div by HVX
                for (int m = 0; m < 32; m++) {
                    // wfq16_t re = wfq8_div(real_buff[m], deno_buff[m]);
                    // wfq16_t im = wfq8_div(imag_buff[m], deno_buff[m]);
                    wfq16_t re = (real_buff[m]<<8)/deno_buff[m];
                    wfq16_t im = (imag_buff[m]<<8)/deno_buff[m];
                    c_v(i, j+m) = re + (((cwfq16_t)im)<<32); 
                } 
            }
            pSrc1 += kk.cols;
            pSrc2 += this->_prob.cols;
        }
        // PRINT_CVMATcwfq8("c_v", c_v, 0, 0, 8, 8);
        // PRINT_CVMATcwfq8("c_v", c_v, _patch_size/2, _patch_size/2, 8, 8);
        wfq15_t q15_tif = wfq15_from_float(train_interp_factor);
        wfq15_t q15_inv_tif = wfq15_const_1() - q15_tif;
        HVX_Vector v_tif = Q6_V_vsplat_R(Q6_R_sath_R(q15_tif)<<16); // set tif to MSB
        HVX_Vector v_inv_tif = Q6_V_vsplat_R(q15_inv_tif<<16); // set inv_tif to MSB

        l2fetch1 = CreateL2pfParam(c_v.colStride(), c_v.colStride(), 1, 0);
        l2fetch2 = CreateL2pfParam(this->_alphaf.colStride(), this->_alphaf.colStride(), 1, 0);
        pSrc1 = c_v.data;
        pSrc2 = this->_alphaf.data;
        L2fetch( (unsigned int)(pSrc1), l2fetch1);
        L2fetch( (unsigned int)(pSrc2), l2fetch2);
        p_pv1 = (HVX_VectorPair*)c_v.data;
        p_pv2 = (HVX_VectorPair*)this->_alphaf.data;
        for (int i = 0; i < kk.rows; i++) {
            if (i < kk.rows-1) {
                L2fetch( (unsigned int)(pSrc1+c_v.cols), l2fetch1);
                L2fetch( (unsigned int)(pSrc2+this->_alphaf.cols), l2fetch2);
            }
            for (int j = 0; j < kk.cols; j += 32) {
                V1_0 = *p_pv1++;
                V0 = Q6_Vw_vasl_VwR(V0, 7); //Q8-->Q15
                V1 = Q6_Vw_vasl_VwR(V1, 7);
                V3_2 = Q6_W_vdeal_VVR(V1, V0, -4); // V2: real, V3: imag 
                // real
                V4 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V2, v_tif);
                // imag
                V5 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V3, v_tif);

                V1_0 = *p_pv2;
                V0 = Q6_Vw_vasr_VwR(V0, 1); // Q16-->Q15
                V1 = Q6_Vw_vasr_VwR(V1, 1);
                V3_2 = Q6_W_vdeal_VVR(V1, V0, -4); // V2: real, V3: imag 
                // real
                V6 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V2, v_inv_tif);
                // imag
                V7 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V3, v_inv_tif);

                V8 = Q6_Vw_vadd_VwVw_sat(V4, V6);
                V9 = Q6_Vw_vadd_VwVw_sat(V5, V7);

                V8 = Q6_Vw_vasl_VwR(V8, 1);
                V9 = Q6_Vw_vasl_VwR(V9, 1);

                V9_8 = Q6_W_vshuff_VVR(V9, V8, -4);

                *p_pv2 = V9_8; 

                p_pv2++;

            }
            pSrc1 += c_v.cols;
            pSrc2 += this->_alphaf.cols;
        }
    }
#endif
#endif
    // printf("-----------update calc: %lluus\n", tic.toc());
    PRINT_CVMATcwfq16("_alphaf", this->_alphaf, 0, 0, 8, 8)
}

CvRect TrackerKCF::detect(
    const CvRect& dst_roi,
    CvMatU8 &dst_image,
    float& peak_value
)
{
    assert(dst_roi.width > 0 && dst_roi.height > 0);

    this->adjustROI(dst_roi, dst_image);
    float cx = this->_roi.x + this->_roi.width / 2.f;
    float cy = this->_roi.y + this->_roi.height / 2.f;

    float res_peak_value = 0.f;
    CvMat_hfq15 features = getFeatures(dst_image, 1.f);
    CvPoint2f res = this->findPixelPeak(
        this->_tmpl,
        features,
        res_peak_value
    );
    peak_value = res_peak_value;

    if (this->_scale_step != 1.f)
    {
        // Test at a smaller scale
        res_peak_value = 0.f;
        features = getFeatures(dst_image, qhmath_div_f(1.f, this->_scale_step));
        CvPoint2f new_res = this->findPixelPeak(
            this->_tmpl,
            features,
            res_peak_value
        );

        if (this->_scale_weight * res_peak_value > peak_value) {
            res = new_res;
            peak_value = this->_scale_weight * res_peak_value;
            this->_scale /= this->_scale_step;
            this->_roi.width /= this->_scale_step;
            this->_roi.height /= this->_scale_step;
        }

        // Test at a bigger scale
        res_peak_value = 0.f;
        features = this->getFeatures(dst_image, this->_scale_step);
        new_res = this->findPixelPeak(
            this->_tmpl,
            features,
            res_peak_value
        );

        if (this->_scale_weight * res_peak_value > peak_value) {
            res = new_res;
            peak_value = this->_scale_weight * res_peak_value;
            this->_scale *= this->_scale_step;
            this->_roi.width *= this->_scale_step;
            this->_roi.height *= this->_scale_step;
        }
    }
    // printf("--------cx=%f,cy=%f,_roi.width=%f,_roi.height=%f,res.x=%f,res.y=%f\n", cx, cy, _roi.width, _roi.height, res.x, res.y);
    this->_roi.x = cx - this->_roi.width / 2.f + ((float) res.x * this->_scale);
    this->_roi.y = cy - this->_roi.height / 2.f + ((float) res.y * this->_scale);

    // printf("---------[%f,%f,%f,%f], peak_value:%f\n", _roi.x, _roi.y, _roi.width, _roi.height, peak_value);
    return CvRect(_roi.x, _roi.y, _roi.width, _roi.height);
}


/////////////////
/** Protected **/
/////////////////
CvPoint2f TrackerKCF::findPixelPeak(CvMat_hfq15 &z, CvMat_hfq15 &x, float& peak_value)
{
    // PRINT_CVMATF32("pixelpeak z", z, 0, 0, 8, 8)
    // PRINT_CVMATF32("pixelpeak x", x, 0, 0, 8, 8)
    // cv::Mat k = this->gaussianCorrelation(x, z);
    // cv::Mat res = FFTTools::real(
    //     FFTTools::fftd(
    //         FFTTools::complexMultiplication(
    //             this->_alphaf,
    //             FFTTools::fftd(k)
    //         ),
    //         true
    //     )
    // );
    CvMat_hfq15 k = this->gaussianCorrelation(x, z);
    // CvMatComplex k_fft = FFTTools::fftd(k, this->_r_twiddles1, this->_r_twiddles2, this->_c_twiddles);
    CvMat_cwfq16 k_fft(this->_patch_size, this->_patch_size);
    _fft.fftd(k.data, k_fft.data);
    // CvMatComplex tmp1 = FFTTools::complexMultiplication( this->_alphaf, k_fft );
    CvMat_cwfq16 tmp1(this->_patch_size, this->_patch_size);
#if 0
    for (int i = 0; i < k_fft.rows; i++) {
        for (int j = 0; j < k_fft.cols; j++) {
            tmp1(i, j) = cwfq16_mul(this->_alphaf(i, j), k_fft(i, j));
        }
    }
#else
    cvmat_cwfq16_mul(this->_alphaf, k_fft, tmp1);
#endif
    // CvMatComplex tmp2 = FFTTools::cifftd( tmp1, this->_c_twiddles);
    // CvMat res = FFTTools::real( tmp2 );
    CvMat_hfq15 res(this->_patch_size, this->_patch_size);
    _fft.rifftd(tmp1.data, res.data);
    PRINT_CVMATfq7("res", res, _patch_size/2, _patch_size/2, 8, 8);
    // PRINT_CVMATF32("pixelpeak res", res, 0, 0, 8, 8)
    //minMaxLoc only accepts doubles for the peak, and integer points for the coordinates
    // cv::Point2i pi;
    // double pv;
    // cv::minMaxLoc(res, NULL, &pv, NULL, &pi);
    // peak_value = (float)pv;
    int16_t minVal, maxVal;
    uint32_t minLocX, minLocY, maxLocX, maxLocY;
    FcvContext::getContext()->dl_fcvMinMaxLocs16(res.data, res.cols, res.rows, res.colStride(), 
                                                &minVal, &maxVal, &minLocX, &minLocY, &maxLocX, &maxLocY);
    peak_value = hfq7_to_float(maxVal);
    // printf("-------peak_value:[%f,%f],[%ld,%ld], [%ld,%ld]\n", hfq7_to_float(minVal), hfq7_to_float(maxVal), minLocX, minLocY, maxLocX, maxLocY);
    //subpixel peak estimation, coordinates will be non-integer
    // cv::Point2f p((float)pi.x, (float)pi.y);
    CvPoint2f pi(maxLocX, maxLocY);
    CvPoint2f p = pi;

    if (pi.x > 0 && pi.x < res.cols-1) {
        p.x += subPixelPeak(res(pi.y, pi.x-1), peak_value, res(pi.y, pi.x+1));
    }

    if (pi.y > 0 && pi.y < res.rows-1) {
        p.y += subPixelPeak(res(pi.y-1, pi.x), peak_value, res(pi.y+1, pi.x));
    }

    p.x -= (res.cols) / 2;
    p.y -= (res.rows) / 2;

    // printf("------p=[%f,%f], %d,%d\n",p.x, p.y, res.cols, res.rows);
    return p;
}

void TrackerKCF::adjustROI(const CvRect& roi, CvMatU8 &image)
{
    float orig_cx = static_cast<float>(roi.x) + static_cast<float>(roi.width) / 2.f;
    float orig_cy = static_cast<float>(roi.y) + static_cast<float>(roi.height) / 2.f;
    if (roi.width >= roi.height)
    {
        this->_roi.width = static_cast<float>(roi.width);
        this->_roi.height = static_cast<float>(roi.width);
    }
    else
    {
        this->_roi.width = static_cast<float>(roi.height);
        this->_roi.height = static_cast<float>(roi.height);
    }
    this->_roi.x = orig_cx - this->_roi.width / 2.f;
    this->_roi.y = orig_cy - this->_roi.height / 2.f;
    if (this->_roi.x + this->_roi.width <= 0) this->_roi.x = -this->_roi.width + 1;
    if (this->_roi.y + this->_roi.height <= 0) this->_roi.y = -this->_roi.height + 1;
    if (this->_roi.x >= image.cols - 1) this->_roi.x = image.cols - 2;
    if (this->_roi.y >= image.rows - 1) this->_roi.y = image.rows - 2;

    float pad_cx = this->_roi.x + this->_roi.width / 2.f;
    float pad_cy = this->_roi.y + this->_roi.height / 2.f;
    float pad_length = std::min(
        this->_roi.width * this->_padding,
        std::min((float)image.rows, (float)image.cols)
    );
    this->_pad_roi.width = pad_length;
    this->_pad_roi.height = pad_length;
    this->_pad_roi.x = pad_cx - pad_length / 2.f;
    this->_pad_roi.y = pad_cy - pad_length / 2.f;

    this->_scale = pad_length / static_cast<float>(this->_patch_size);
    // printf("_roi:[%f,%f,%f,%f],_pad_roi:[%f,%f,%f,%f], _scale=%f\n", _roi.x, _roi.y, _roi.width, _roi.height, _pad_roi.x, _pad_roi.y, _pad_roi.width, _pad_roi.height, _scale);
}

CvMat_hfq15 TrackerKCF::gaussianCorrelation(CvMat_hfq15 & x1, CvMat_hfq15 & x2)
{
    TicToc tic;
    // cv::Mat c(this->_patch_size, this->_patch_size);
    // c.zeros();
    // cv::mulSpectrums(FFTTools::fftd(x1), FFTTools::fftd(x2), c, 0, true);
    TicToc tic1;
    CvMat_cwfq16 x1_c(this->_patch_size, this->_patch_size);
    _fft.fftd(x1.data, x1_c.data);
    // printf("-----------fft: %lluus\n", tic1.toc());
    CvMat_cwfq16 x2_c(this->_patch_size, this->_patch_size); 
    _fft.fftd(x2.data, x2_c.data);
    tic1.tic();
    
    PRINT_CVMATcwfq18("x1_c", x1_c, 0, 0, 8, 8);
    PRINT_CVMATcwfq18("x1_c", x1_c, _patch_size/2, _patch_size/2, 8, 8);
    CvMat_cwfq16 c(this->_patch_size, this->_patch_size);
#if 0
    for (int i = 0; i < x2_c.rows*x2_c.cols; i++) {
        // c(i) = cwfq16_mul_conj(x1_c(i), x2_c(i));
        c(i) = cwfq16_mul_conj2(x1_c(i), x2_c(i));
    }
#else
    if (this->_patch_size == 64) {
        // cvmat_cwfq16_mul_conj_64x64(x1_c, x2_c, c);
        cvmat_cwfq16_mul_conj(x1_c, x2_c, c);
        PRINT_CVMATcwfq11("spectrum", c, 0, 0, 8, 8);
        PRINT_CVMATcwfq11("spectrum", c, _patch_size/2, _patch_size/2, 8, 8);
    } else {
        cvmat_cwfq16_mul_conj(x1_c, x2_c, c);
        PRINT_CVMATcwfq7("spectrum", c, 0, 0, 8, 8);
        PRINT_CVMATcwfq7("spectrum", c, _patch_size/2, _patch_size/2, 8, 8);
    }
#endif
    // memcpy(debug_buf, c.data, c.rows*c.cols*sizeof(cwfq16_t));
    // printf("-----------add conj: %lluus\n", tic1.toc());
    // int ret = qhblas_cf_vector_dot_acf(x1_c.data, x2_c.data, c.data, c.rows*c.cols);
    // assert(ret == 0);

    // c = FFTTools::fftd(c, true);
    tic1.tic();
    CvMat_hfq15  c_inv(this->_patch_size, this->_patch_size);
    _fft.rifftd(c.data, c_inv.data);
    // printf("-----------r2difft: %lluus\n", tic1.toc());
    tic1.tic();
    if (this->_patch_size == 64) {
        rearrange_64(c_inv);
        PRINT_CVMATfq8("c_real", c_inv, _patch_size/2, _patch_size/2, 8, 8);
    } else if (this->_patch_size == 128) {
        rearrange_128(c_inv);
        PRINT_CVMATfq6("c_real", c_inv, _patch_size/2, _patch_size/2, 8, 8);
    }
    // printf("-----------rearrange: %lluus\n", tic1.toc());
    // CvMatF32 c_real = FFTTools::real(c);
    // float patch_step = this->_patch_size*this->_patch_size;
    // cv::Mat d; 
    // cv::max(((cv::sum(x1.mul(x1))[0] + cv::sum(x2.mul(x2))[0]) - 2.f * c) / patch_step, 0, d);
    // cv::Mat k;
    // cv::exp((-d / (this->_sigma * this->_sigma)), k);
    // return k;
    tic1.tic();
#if 1 // result isn't right
    int32_t x1_sum = 0;
    int32_t x2_sum = 0;
    cvmat_hfq15_dot(x1, x1, &x1_sum);
    cvmat_hfq15_dot(x2, x2, &x2_sum);
    // printf("------x1_sum=%f, x2_sum=%f, _sigma=%f\n", wfq15_to_float(x1_sum), wfq15_to_float(x2_sum), this->_sigma);
    int32_t sum = (x1_sum+x1_sum); // Q15
#else
    wfq15_t x1_sum = 0;
    wfq15_t x2_sum = 0;
    for (int i = 0; i < x1.rows * x1.cols; i++) {
        x1_sum += hfq15_mul(x1(i), x1(i));
        x2_sum += hfq15_mul(x2(i), x2(i));
    }
    wfq15_t sum = x1_sum + x2_sum;
    printf("------x1_sum=%f, x2_sum=%f, _sigma=%f\n", wfq15_to_float(x1_sum), wfq15_to_float(x2_sum), this->_sigma);
#endif
    // printf("-----------vec dot: %lluus\n", tic1.toc());
    tic1.tic();
    CvMat_hfq15 k(this->_patch_size, this->_patch_size);
#if 0
    wfq15_t patch_step_inv = wfq15_from_float(qhmath_div_f(1.f, this->_patch_size*this->_patch_size)); // Q15
    for (int i = 0; i < k.rows; i++) {
        for (int j = 0; j < k.cols; j++) {
            wfq15_t c_real = Q6_R_asl_RR_sat(c_inv(i, j), 8); //Q7-->Q15 
            c_real = wfq15_mul(c_real, wfq15_from_int(2));
            wfq15_t v = wfq15_mul(sum - c_real, patch_step_inv);
            // v = std::max(v, 0);
            if (v&0x80000000) {
                v = hfq15_const_e();
            }
            v = wfq15_mul(v, this->_neg_sigma2);
            uint32_t v_exp = qhmath_exp_h(v>>4); // Q15--> Q4.11 --> uQ16.16
            // 0. < v_exp < 1.0
            k(i, j) = (int16_t)(v_exp>>1);   // uQ16.16 --> Q15 
        }
    }
#else
    {
        CvMat_hfq15 tmp(this->_patch_size, this->_patch_size);
        wfq15_t patch_step_inv = wfq15_from_float(qhmath_div_f(1.f, this->_patch_size*this->_patch_size)); 
        // wfq15_t const_2 = wfq15_from_int(2);
        // HVX_Vector v_const_2 = Q6_V_vsplat_R(const_2);
        HVX_Vector v_patch_step_inv = Q6_V_vsplat_R(patch_step_inv<<16); // set patch_step_inv to MSB
        HVX_Vector v_neg_sigma2 = Q6_V_vsplat_R(this->_neg_sigma2); 
        HVX_Vector v_sum = Q6_V_vsplat_R(sum);
        HVX_Vector v_zero = Q6_V_vsplat_R(1);// Q6_V_vzero();
        long long l2fetch = CreateL2pfParam(c_inv.colStride(), c_inv.cols, 1, 0);
        hfq15_t *pSrc = c_inv.data;
        L2fetch( (unsigned int)(pSrc), l2fetch);
        HVX_VP V1__0, V3__2;
        HVX_Vector *p_v1 = (HVX_Vector*)c_inv.data;
        HVX_Vector *p_v2 = (HVX_Vector*)tmp.data;
        uint32_t shift = (this->_patch_size == 64) ? 7 : 9;
        for (int i = 0; i < c_inv.rows; i++) {
            if (i < c_inv.rows-1) {
                L2fetch( (unsigned int)(pSrc+c_inv.cols), l2fetch);
            }
            for (int j = 0; j < c_inv.cols; j += 64) {
                V0 = *p_v1++;
                // V1_0 = Q6_Ww_vsxt_Vh(V0);
                V1_0 = Q6_Ww_vunpack_Vh(V0);
                V0 = Q6_Vw_vasl_VwR(V0, shift); // Q6-->Q15, Q8-->Q15
                V1 = Q6_Vw_vasl_VwR(V1, shift);
                // c_inv*2
                V2 = Q6_Vw_vadd_VwVw(V0, V0);
                V3 = Q6_Vw_vadd_VwVw(V1, V1);
                V2 = Q6_Vw_vsub_VwVw_sat(v_sum, V2);
                V3 = Q6_Vw_vsub_VwVw_sat(v_sum, V3);
                V2 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V2, v_patch_step_inv);
                V3 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V3, v_patch_step_inv);
                V2 = Q6_Vw_vmax_VwVw(V2, v_zero);
                V3 = Q6_Vw_vmax_VwVw(V3, v_zero);
                V2 = Q6_Vw_vasl_VwR(V2, 16); // set to MSB
                V3 = Q6_Vw_vasl_VwR(V3, 16);
                V2 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(v_neg_sigma2, V2);
                V3 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(v_neg_sigma2, V3);
                V0 = Q6_Vh_vpack_VwVw_sat(V3, V2);
                *p_v2++ = V0;
            }
            pSrc += c_inv.cols;
        }
        // in: Q15, out: Q13
        qhmath_hvx_exp_ah(tmp.data, k.data, k.rows*k.cols);
        // Q13 --> Q15
        pSrc = k.data;
        L2fetch( (unsigned int)(pSrc), l2fetch);
        p_v1 = (HVX_Vector*)k.data;
        for (int i = 0; i < k.rows; i++) {
            if (i < k.rows-1) {
                L2fetch( (unsigned int)(pSrc+k.cols), l2fetch);
            }
            for (int j = 0; j < k.cols; j += 64) {
                V0 = *p_v1;
                V0 = Q6_Vh_vasl_VhR(V0, 2);
                *p_v1++ = V0;
            }
            pSrc += k.cols;
        } 
    }
#endif
    // printf("-------gauss correc: %lluus\n", tic1.toc());
    PRINT_CVMATfq15("corr", k, _patch_size/2, _patch_size/2, 8, 8);
    return k;
}

CvMat_hfq15 TrackerKCF::getFeatures(const CvMatU8& image, float scale_adjust)
{
    TicToc tic;
    int cx = static_cast<int>(this->_pad_roi.x + this->_pad_roi.width / 2.f);
    int cy = static_cast<int>(this->_pad_roi.y + this->_pad_roi.height / 2.f);
    CvRect extracted_roi;
    extracted_roi.width = static_cast<int>(this->_pad_roi.width * scale_adjust);
    extracted_roi.height = static_cast<int>(this->_pad_roi.height * scale_adjust);
    extracted_roi.x = cx - extracted_roi.width / 2;
    extracted_roi.y = cy - extracted_roi.height / 2;

    // printf("-------extracted_roi:[%ld,%ld,%ld,%ld]\n", extracted_roi.x, extracted_roi.y, extracted_roi.width, extracted_roi.height); 
    CvMatU8 z = RectTools::subwindow(image, extracted_roi, CV_BORDER_REPLICATE);
    if (z.cols != this->_patch_size || z.rows != this->_patch_size) {
    //     cv::resize(z, z, cv::Size(this->_patch_size, this->_patch_size));
        TicToc tic;
        CvMatU8 z1(this->_patch_size, this->_patch_size);
        // Fixme: cols must be multiply 8
        // Fixme: is always scale down?
        #if 0
        FcvContext::getContext()->dl_fcvScaleu8(z.data, z.cols, z.rows, z.cols,
                                                    z1.data, z1.cols, z1.rows, z1.cols,
                                                    FASTCV_INTERPOLATION_TYPE_BILINEAR);
        #else
        FcvContext::getContext()->dl_fcvScaleDownBLu8(z.data, z.cols, z.rows, z.colStride(), z1.data, z1.cols, z1.rows, z1.colStride());
        #endif
        // printf("-----------resize: [%d,%d]-----%lluus-----\n",z.rows, z.cols, tic.toc());
        z = z1;
    } 
    // PRINT_CVMATU8("z", z);
#if 0
    CvMatF32 FeaturesMap = RectTools::getGrayImage(z);
    // FeaturesMap -= 0.5f;
    // FeaturesMap = this->_hann.mul(FeaturesMap);
    CvMatF32 tmp(FeaturesMap.rows, FeaturesMap.cols);
    FcvContext::getContext()->dl_fcvAddScalarf32(FeaturesMap.data, FeaturesMap.cols, FeaturesMap.rows, FeaturesMap.colStride(),
                                                -0.5f,
                                                tmp.data, tmp.colStride());
    FcvContext::getContext()->dl_fcvElementMultiplyf32(this->_hann.data, this->_hann.cols, this->_hann.rows, this->_hann.colStride(),
                                                    tmp.data, tmp.colStride(),
                                                    FeaturesMap.data, FeaturesMap.colStride());
#else
    CvMat_hfq15 FeaturesMap(z.rows, z.cols);
    hfq15_t scale = hfq15_from_float(qhmath_div_f(1, 255.f));
    hfq15_t c_offset = hfq15_from_float(0.5f);
#if 0
    for (int i = 0; i < z.rows; i++) {
        for (int j = 0; j < z.cols; j++) {
            // FeaturesMap(i, j) = (static_cast<float>(z(i,j))*scale-0.5f)*this->_hann(i,j);
            wfq15_t v = wfq15_from_int(z(i, j)); // Q15
            v = wfq15_mul(v, scale);
            v -= c_offset;
            FeaturesMap(i, j) = wfq15_mul(v, this->_hann(i, j));
        }
    }
#else
    long long l2fetch1 = CreateL2pfParam(z.colStride(), 128, 1, 0);
    long long l2fetch2 = CreateL2pfParam(this->_hann.colStride(), 128, 1, 0);
    uint8_t *pSrc1 = z.data;
    hfq15_t *pSrc2 = this->_hann.data;
    hfq15_t *pdst = FeaturesMap.data;
    HVX_Vector v_c_offset = Q6_V_vsplat_R(c_offset);
    HVX_Vector v_scale = Q6_V_vsplat_R(scale);
    v_scale = Q6_Vw_vasl_VwR(v_scale, 16);
    HVX_VectorPair vp_zero = Q6_W_vzero();
    L2fetch( (unsigned int)(pSrc1), l2fetch1);
    L2fetch( (unsigned int)(pSrc2), l2fetch2);
    HVX_VP V1__0, V3__2, V5__4, V7__6, V9__8;
    int rows = (z.cols == 64) ? (z.rows/2) : z.rows;
    for (int i = 0; i < rows; i++) {
        if (i < (rows-1)) {
            L2fetch( (unsigned int)(pSrc1+128), l2fetch1);
            L2fetch( (unsigned int)(pSrc2+128), l2fetch2);
        }
        HVX_Vector tmp = *(HVX_Vector*)pSrc1;
        V1_0 = Q6_Wuh_vunpack_Vub(tmp);
        V3_2 = Q6_Ww_vunpack_Vh(V0);
        V5_4 = Q6_Ww_vunpack_Vh(V1);
        V2 = Q6_Vw_vasl_VwR(V2, 15);
        V3 = Q6_Vw_vasl_VwR(V3, 15);
        V4 = Q6_Vw_vasl_VwR(V4, 15);
        V5 = Q6_Vw_vasl_VwR(V5, 15);
        V2 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V2, v_scale);
        V3 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V3, v_scale);
        V4 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V4, v_scale);
        V5 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V5, v_scale);
        V2 = Q6_Vw_vsub_VwVw_sat(V2, v_c_offset);
        V3 = Q6_Vw_vsub_VwVw_sat(V3, v_c_offset);
        V4 = Q6_Vw_vsub_VwVw_sat(V4, v_c_offset);
        V5 = Q6_Vw_vsub_VwVw_sat(V5, v_c_offset);
        V1_0 = *(HVX_VectorPair*)pSrc2;
        V7_6 = Q6_Ww_vunpackoor_WwVh(vp_zero, V0);
        V9_8 = Q6_Ww_vunpackoor_WwVh(vp_zero, V1);
        V2 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V2, V6);
        V3 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V3, V7);
        V4 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V4, V8);
        V5 = Q6_Vw_vmpyo_VwVh_s1_rnd_sat(V5, V9);
        V0 = Q6_Vh_vpack_VwVw_sat(V3, V2);
        V1 = Q6_Vh_vpack_VwVw_sat(V5, V4);
        *(HVX_VectorPair*)pdst = V1_0; 
        pdst += 128;
        pSrc1 += 128;
        pSrc2 += 128;
    }
#endif
    // printf("----------FeaturesMap: %lluus\n", tic.toc());
#endif
    PRINT_CVMATfq15("FeaturesMap", FeaturesMap, _patch_size/2, _patch_size/2, 8, 8)
    return FeaturesMap;
}

float TrackerKCF::subPixelPeak(float left, float center, float right)
{
    float divisor = 2.f * center - right - left;

    if (divisor == 0.f)
        return 0.f;
    
    // return 0.5f * (right - left) / divisor;
    return qhmath_div_f(0.5f * (right - left), divisor);
}

void TrackerKCF::createHanningMats()
{
    // cv::Mat hann1t = cv::Mat(cv::Size(this->_patch_size, 1), CV_32F, cv::Scalar(0));
    // cv::Mat hann2t = cv::Mat(cv::Size(1, this->_patch_size), CV_32F, cv::Scalar(0)); 
    // for (int i = 0; i < hann1t.cols; i++) {
    //     hann1t.at<float > (0, i) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann1t.cols - 1)));
    // }
    // for (int i = 0; i < hann2t.rows; i++) {
    //     hann2t.at<float > (i, 0) = 0.5 * (1 - std::cos(2 * 3.14159265358979323846 * i / (hann2t.rows - 1)));
    // }
    // this->_hann = hann2t * hann1t;
    CvMatF32 hann1t(1, this->_patch_size);
    CvMatF32 hann2t(this->_patch_size, 1);
    CvMat_hfq15 hann(this->_patch_size, this->_patch_size);
    for (int i = 0; i < hann1t.cols; i++) {
        hann1t(0, i) = 0.5 * (1 - qhmath_cos_f(2 * 3.14159265358979323846 * i / (hann1t.cols - 1)));
    }
    for (int i = 0; i < hann2t.rows; i++) {
        hann2t(i, 0) = 0.5 * (1 - qhmath_cos_f(2 * 3.14159265358979323846 * i / (hann2t.rows - 1)));
    }
    #if 1
    for (int i = 0; i < this->_patch_size; i++) {
        float v1 = hann2t(i, 0); 
        for (int j = 0; j < this->_patch_size; j++) {
            float v2 = hann1t(0, j);
            hann(i, j) = hfq15_from_float(v1*v2); 
        }
    } 
    #else
    FcvContext::getContext()->dl_fcvMatrixMultiplyf32(hann2t.data, hann2t.cols, hann2t.rows, hann2t.colStride(), 
                                                    hann1t.data, hann1t.cols, hann1t.colStride(), 
                                                    hann.data, hann.colStride());
    #endif
    this->_hann = hann;

    PRINT_CVMATfq15("hann", this->_hann, _patch_size/2, _patch_size/2, 8, 8)
}

void TrackerKCF::createGaussianPeak()
{
    CvMat_hfq15 res(this->_patch_size, this->_patch_size);

    int syh = this->_patch_size / 2;
    int sxh = this->_patch_size / 2;

    float output_sigma = qhmath_sqrt_f((float) this->_patch_size * this->_patch_size) / this->_padding * this->_output_sigma_factor;
    float mult = qhmath_div_f(-0.5, output_sigma * output_sigma);

    for (int i = 0; i < this->_patch_size; i++)
    {
        for (int j = 0; j < this->_patch_size; j++)
        {
            int ih = i - syh;
            int jh = j - sxh;
            res(i, j) = hfq15_from_float(qhmath_exp_f(mult * (float) (ih * ih + jh * jh)));
        }
    }
    // PRINT_CVMATfq15("res", res, 64, 64, 16, 16);
    // this->_prob = FFTTools::fftd(res, this->_r_twiddles1, this->_r_twiddles2, this->_c_twiddles);
    this->_prob.resize(this->_patch_size, this->_patch_size);
    _fft.fftd(res.data, this->_prob.data);
    PRINT_CVMATcwfq16("_prob", this->_prob, 0, 0, 8, 8);
    PRINT_CVMATcwfq16("_prob", this->_prob, _patch_size/2, _patch_size/2, 8, 8);
}

// } // end namespace handtrack